#include "PhysicsSystem.h"
#include "CollisionHelper.h"
#include "HUDManager.h"

PhysicsSystem* PhysicsSystem::instance = 0;

PhysicsSystem::PhysicsSystem(void)	{

}

PhysicsSystem::~PhysicsSystem(void)	{
	for (int i = 0; i < allSprings.size(); i++) {
		delete allSprings[i];
	}
}

void	PhysicsSystem::Update(float msec) {	

	if(!HUDManager::GetInstance()->GetPaused() && HUDManager::GetInstance()->GetRunning())
	{
		allNodesMutex.lock();
		copiedNodes.clear();
		copiedNodes = allNodes;
		allNodesMutex.unlock();

		if (copiedNodes.size())
			BroadPhaseCollisions();

		/*for(vector<Constraint*>::iterator i = allSprings.begin(); i != allSprings.end(); ++i) {
			(*i)->Update(msec);
		}*/

		for (vector<PhysicsNode*>::iterator i = copiedNodes.begin(); i != copiedNodes.end(); ++i) {
			//if something is deleted in all nodes, just skip this frames update..... or not?
			//TODO: evaluate if the skipping is necessary ---- Kan
			if (copiedNodes.size() != allNodes.size())
				break;
			(*i)->Update(msec);
			// ---------- Modified by Monica --------------- //
			float zMovement = player->getAmountMovementOnZAxis();
			if ((*i)->GetPosition().z < zMovement) {
				// -------------------------------------------- //
				(*i)->flipDespawn();
			}
		}

		// -------- Added by Monica ----- //
		//now update the player's physicsnode
		//this was necessary because the threading caused problems in the normal update in mygame class
		//it is important that the player update will take place before the terrain one
		player->UpdatePlayer(msec);
		//playerPhysics->Update(msec);

		terrain->UpdatePhysicsTerrain(msec);
		// -------------------------;----- //
	}
}

/* Method used to cull the collision detection and pass it to narrowphase
* Return: None
* Parameters: Whether we are using culling here
* Author: Kan Tang
*/
void	PhysicsSystem::BroadPhaseCollisions(const bool& useCulling)
{
	if (useCulling)
	{
		//sort all the nodes on x axis with the bounding radius
		vector<PhysicsNode*> narrowPhaseNodes = vector<PhysicsNode*>(copiedNodes);
		sort(narrowPhaseNodes.begin(), narrowPhaseNodes.end(), [](const PhysicsNode* a, const PhysicsNode* b){ return a->GetZBegin() < b->GetZBegin(); });

		//------ Added by Monica ------- //
		if (narrowPhaseNodes.size() > 0) {
		// ------------------------------ //

			for (vector<PhysicsNode*>::iterator i = narrowPhaseNodes.begin(); i != narrowPhaseNodes.end() - 1; ++i)
			{
				PhysicsNode& first = *(*i);
				CollisionVolume* fv = first.GetCollisionVolume();
				if (!fv) continue;
				for (vector<PhysicsNode*>::iterator k = i + 1; k != narrowPhaseNodes.end(); ++k)
				{
					PhysicsNode& second = *(*k);
					CollisionVolume* sv = second.GetCollisionVolume();
					if (!sv) continue;
					//if both node is not unknown type, then go on to sort and sweep.
					if (second.GetZBegin() > first.GetZEnd())
						break;
					NarrowPhaseCollisions(first, second);
				}
			}
		}
	}
	else
	{
		for (int i = 0; i < copiedNodes.size() - 1; i++)
		{
			PhysicsNode& first = *copiedNodes[i];
			CollisionVolume* fv = first.GetCollisionVolume();
			if (!fv) continue;
			for (int j = i + 1; j < copiedNodes.size(); j++)
			{
				PhysicsNode& second = *copiedNodes[j];
				CollisionVolume* sv = second.GetCollisionVolume();
				if (!sv) continue;
				NarrowPhaseCollisions(first, second);
			}
		}
	}
}

/* Method used to determine if two objects are collided
* Modified: Kan Tang
* Return: None
* Parameters: Whether we are using culling here
* Author: ---
*/
void	PhysicsSystem::NarrowPhaseCollisions(PhysicsNode& first, PhysicsNode& second) {
	CollisionVolume* fv = first.GetCollisionVolume();
	CollisionVolume* sv = second.GetCollisionVolume();
	switch (fv->GetType()) {
	case COLLISION_VOL_SPHERE:
		switch (sv->GetType()) {
		case COLLISION_VOL_SPHERE:
			CollisionData data;
			if (CollisionHelper::SphereSphereCollision(first, second, &data)) {
				CollisionHelper::AddCollisionImpulse(first, second, data);
			}
			return;
		}
		//<----- CHECK THE BLOODY CASE WHEN DONE ----->
	case COLLISION_VOL_OBB:
		switch (sv->GetType())
		{
		case COLLISION_VOL_OBB:
			//CollisionData data;
			if (CollisionHelper::OBBCollision(first, second))
			{
				//<<<----- ADD SPECIFIC COLLISION RESPONSES HERE ----->>>
				if (first.GetCollisionVolume()->GetSubType() == PLAYER && second.GetCollisionVolume()->GetSubType() == BUFF)
				{
					player->Buff();
					player->getSubject()->notify(player);
					second.flipDespawn();
					//cout << ".....collision with buff......" << endl;
				}
				else if (first.GetCollisionVolume()->GetSubType() == PLAYER && second.GetCollisionVolume()->GetSubType() == DEBUFF)
				{
					player->Debuff();
					//player->getSubject()->notify(player);
					second.flipDespawn();
				}
				else if (first.GetCollisionVolume()->GetSubType() == PLAYER && second.GetCollisionVolume()->GetSubType() ==RUSH_BUFF)
				{
					player->SetStateInCubeRobot(RUSH);
					player->getSubject()->notify(player);
					second.flipDespawn();
				}
				else if (first.GetCollisionVolume()->GetSubType() == PLAYER && second.GetCollisionVolume()->GetSubType() == FLY_BUFF)
				{
					player->SetStateInCubeRobot(FLY);
					player->getSubject()->notify(player);
					second.flipDespawn();
				}
				return;
			}
		}
	}
}

void	PhysicsSystem::AddNode(PhysicsNode* n) {
	allNodesMutex.lock();
	allNodes.push_back(n);
	allNodesMutex.unlock();
}

void	PhysicsSystem::RemoveNode(PhysicsNode* n) {
	for(vector<PhysicsNode*>::iterator i = allNodes.begin(); i != allNodes.end(); ++i) {
		if((*i) == n) {
			allNodesMutex.lock();
			allNodes.erase(i);
			allNodesMutex.unlock();
			return;
		}
	}
}

void	PhysicsSystem::AddConstraint(Constraint* s) {
	allSprings.push_back(s);
}

void	PhysicsSystem::RemoveConstraint(Constraint* c) {
	for(vector<Constraint*>::iterator i = allSprings.begin(); i != allSprings.end(); ++i) {
		if((*i) == c) {
			allSprings.erase(i);
			return;
		}
	}
}

void	PhysicsSystem::AddDebugDraw(DebugDrawer* d) {
	allDebug.push_back(d);
}

void	PhysicsSystem::RemoveDebugDraw(DebugDrawer* d) {
	for(vector<DebugDrawer*>::iterator i = allDebug.begin(); i != allDebug.end(); ++i) {
		if((*i) == d) {
			allDebug.erase(i);
			return;
		}
	}
}

void    PhysicsSystem::DrawDebug() {
	for(vector<DebugDrawer*>::iterator i = allDebug.begin(); i != allDebug.end(); ++i) {
		(*i)->DebugDraw();
	}
}